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[57] ABSTRACT 

The invention is embodied in a method of controlling a robot 
manipulator moving toward a target frame F 0 with a target 
velocity v 0 including a linear target velocity v and an angular 
target velocity co 0 to smoothly and continuously divert the 
robot manipulator to a subsequent frame by determining 
a global transition velocity v 1? the global transition velocity 
including a linear transition velocity v x and an angular 
transition velocity 0 ) l5 defining a blend time interval 2t 0 
within which the global velocity of the robot manipulator is 
to be changed from a global target velocity v 0 to the global 
transition velocity v : and dividing the blend time interval 
2 t 0 into discrete time segments 5t. During each one of the 
discrete time segments 5t of the blend interval 2 t 0 , a blended 
global velocity v of the manipulator is computed as a blend 
of the global target velocity v 0 and the global transition 
velocity v 1? the blended global velocity v including a 
blended angular velocity co and a blended linear velocity v, 
and then, the manipulator is rotated by an incremental 
rotation corresponding to an integration of the blended 
angular velocity ca over one discrete time segment 5t. 
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1 

TASK SPACE ANGULAR VELOCITY 
BLENDING FOR REAL-TIME TRAJECTORY 
GENERATION 

ORIGIN OF THE INVENTION 5 

The invention described herein was made in the perfor- 
mance of work under a NASA contract, and is subject to the 
provisions of Public Law 96-517 (35 USC 202) in which the 
contractor has elected not to retain title. 10 

BACKGROUND OF THE INVENTION 

1. Technical Field 

The invention relates to a compliant motion control 15 
system for controlling a robot using angular velocity blend- 
ing in task space in performing specific tasks. 

2. Background Art 

The specification below makes reference to the following 
publications by number: 20 

References 

[1] M. Brady and others (editors). Robot Motion: Plan- 
ning and Control. MIT Press, Cambridge Mass., 1982. 25 

[2] J. Canny. Collision Detection for Moving Polyhedra. 
IEEE Transactions on Pattern Analysis and Ma 

[3] J. Craig. Introduction to Robotics: Mechanics and 

control Addison- Wesley, Reading, Mass., 1986. 30 

[4] H. Goldstein. Classical Mechanics . Addison- Wesley, 
Reading, Mass., 1980. 

[5] C. Lin and P. Chang. Formulation and Optimization of 
Cubic Polynomial Joint Trajectories for Industrial 
Robots. IEEE Transactions on Automatic Control, 35 
28(12): 1066-1073, 1983. 

[6] J. Lloyd and V. Hayward. Real-time Trajectory Gen- 

eration Using Blend Functions. In IEEE International 
Conference on Robotics and Automation, Sacramento, 
Calif., April 1991. 40 

[7] M. Mujtaba. Discussion of Trajectory Calculation 
Methods. Stanford University, Artificial Intelligence 
Laboratory, AIM 285.4, 1977. 

[8] R. Paul. Robot Manipulators: Mathematics, Program- 
ming and Control. MIT Press, Cambridge, Mass., 1981. 

[9] R. Paul. Manipulator Cartesian Path Control, pages 
245-263. MIT Press, Cambridge, Mass., 1982. 

[10] R. Paul and H. Zhang. Robot Motion Trajectory 
Specification and Generation. In Second International 50 
Symposium on Robotics Research, Kyoto, Japan, 
August 1984. 

[11] R. Rosenberg and D. Kamopp. Introduction to Physi- 
cal System Dynamics. McGraw-Hill, New York, 1983. 

[12] H. Seraji and R. Colbaugh. Improved Configuration 55 
Control for Redundant Robots. Journal of Robotics 
Systems, 7(6), 1990. 

[13] R. Taylor. Planning and Execution of Straight Line 
Manipulator Trajectories, pages 265-286. MIT Press, 
Cambridge, Mass., 1982. 

[14] S. Thompson and R. Patel. Formulation of Joint 

Trajectories for Industrial Robots Using B-Splines. 
IEEE Transactions on Industrial Electronics, 34(2): 
192-199, 1987. 65 

[15] D. Whitney. Resolved Motion Rate Control of 
Manipulators and Human Pro theses. IEEE Tr ansae- 


2 

tions on Man-Machine Systems, 10(2): 49-53, June 

1969. 

1 Introduction 

Just as manipulator control can be effectively accom- 
plished in joint space or task space, trajectories for the 
manipulator can also be specified in joint or task space. 
Typically, the trajectory is specified in the same space in 
which the controller is working. However, conversion tech- 
niques can be used to translate the specified trajectory to the 
control space. For instance, inverse kinematics applied to a 
task space trajectory will provide setpoints to a joint space 
controller. Since task space trajectory specification is usually 
considered most useful (especially with task space control), 
the converse translation of a joint space trajectory to task 
space is uncommon. 

Joint space trajectory generation is straightforward since 
each joint may be treated independently [8, 1, 3]. Typically, 
motion between specified joint values is dictated with a 
third, fourth, or fifth order polynomial. Some extension and 
optimization of this technique have been proposed [5, 14]. 

Task space trajectory generation has been addressed more 
extensively, because of the complexity inherent in it. Whit- 
ney proposed Resolved Rate control [15] to easily enable 
straight line motion or constant axis rotation of an end 
effector. However, this technique does not inherently 
address extended trajectory generation considerations. Fore- 
most among these is the problem of blending changes in end 
effector orientation. Paul [8, 10] proposed blending of the 
Euler angles describing the relations of the initial and final 
frames to the intermediate one. This method blends one 
orientation to the next, but the path generated is not intu- 
itively obvious. Worse, he proposes changing one Euler 
angle with a different blend profile from the others. Alter- 
natively, Canny [2] utilizes quaternions to describe orienta- 
tion. However, since he was addressing a different problem 
(collision detection), he does not discuss the issues of 
blending the quaternions. Craig [3] utilizes the similar 
angle-axis formulation, but represents the orientation of 
each via frame with respect to the world frame, not the 
previous frame as Paul had done. Thus, the blend of orien- 
tation parameters will produce a motion path that is depen- 
dent on the relation of the via frames to the world frame, not 
just their relation to each other. Finally, Lloyd and Hayward 
[6] developed an elegant method for creating variable posi- 
tion blend paths, but do not show an extension of the method 
for orientations. 

As will be seen, Taylor [13] has proposed a scheme that 
provides smooth, intuitive, and repeatable position and 
orientation blends. Its major drawback is computational 
complexity. This paper presents a velocity based method that 
achieves the same results with a simpler formulation and 
significantly reduced computation time. 

The next section presents the terminology employed for 
the solution description. Section 3 presents the proposed 
velocity blending formulation and described possible blend 
profile functions. Section 4 quickly discusses position path 
blending. Orientation blending is extensively discussed in 
Section 5, where Taylor’s method is reviewed, angular 
velocity blending is presented, and the second order differ- 
ence between them is analyzed. Sections 6 and 7 discuss 
implementational considerations and computational costs 
associated with the algorithms and show why velocity 
blending is preferable. Finally, Section 8 describes the 
results of simulation and real-time implementation. 

2 Velocity Blending Terminology 

A task frame is defined as the set containing the rotation 
matrix that specifies the end effector orientation, r, the end 



5,602,968 


3 

effector position, p, other scalar configuration control 
parameters (e.g. arm angle \\f [12]), and the transit to this arm 
pose, T. Thus, 


4 

computation steps and is therefore faster than prior art 
processes. 


F ^{ R,-, p*, Vi, T 


5 


( 1 ) 


Typically the end effector orientation is specified by a 
rotation matrix composed of the vectors defining the end 
effector orientation with respect to the stationary world 
frame [8]. 10 


Ri — ( n r °i, a il (2) 

To specify a frame, rotation matrix, or vector with respect to 15 
another frame, the former is proceeded with a superscript. 

For instance, a frame, rotation, or vector with respect to the 
world frame is denoted by “F, “r, “p. 

In between two sequential frames, the desired linear 
velocity of the end effector is simply the difference in 
position over time: 20 

A p Pi~ Pi - 1 (3) 

V, '“ At ~ T t 

The angular velocity is obtained from the equivalent angle- 
axis formulation for a rotation from one frame to another [3]: 25 


“i = K tyJTi 

(4) 

ki sin (j ),• = (bh x m + o*_i x o, + a^\ x ad 

(5) 

cos (J)j = ~- (n t _i ■ «, + Oi-i ■ Oi + flj-i • di - 1) 

(6) 

where motion at velocity co for time At causes a rotation of: 

■RfcoAr] = -RM) = 

(7) 

F k x kyVp — k z S& 

kxk z Vq + kySty "I 


kjky Vty + kykyVif + kykfVfy — kjSty 


^ kySty kykjty fa + kxSfy k^k^V ^ j 

with Sq= sin^, C^=cos^ and V^l-cos^. 

If the magnitude of Equation (5) is zero, the direction of 
k is indeterminant. If Equation (6) equals +1, then the 
orientations of the successive frames are identical, and 
co^tO, 0, 0]. Otherwise, Equation (6) equals -1, and k must 
be determined from the columns of the homogeneous trans- 
form From the first column of Equation (7) 

we have: 


30 


35 


40 


45 


k x = 


*ii + l 


( 8 ) 


50 


ky- 


*21 


kz 


*31 

"’2*7 


(9) 

( iq ) 55 


If k.=0, another column must be used, and a similar set of 
solutions calculated. 

Finally, Equations (3) and (4) may be incorporated into a 
global definition of frame velocity: 


V=[V,(0, y] (11) 

where the scalar velocity is also calculated as in Equation 
(3). ^ 65 

It is the object of the present invention to provide a 
velocity blending robot control process which requires fewer 


SUMMARY OF THE INVENTION 

The invention is embodied in a method of controlling a 
robot manipulator moving toward a target frame F 0 with a 
target velocity v 0 including a linear target velocity v and an 
angular target velocity co 0 to smoothly divert the robot 
manipulator to a subsequent frame F 1? the target frame being 
associated with a target transition time T 0 and the subse- 
quent frame being associated with a subsequent transition 
time Tj, by determining a global transition velocity v l 
necessary to move the manipulator from the target frame F 0 
to the subsequent frame Fj within the subsequent transition 
time Tj, the global transition velocity including a linear 
transition velocity Vj and an angular transition velocity co^ 
defining a blend time interval 2t 0 within which the global 
velocity of the robot manipulator is to be changed from the 
global target velocity v 0 to the global transition velocity v 1 
and dividing the blend time interval 2x 0 into discrete time 
segments 8t. During each one of the discrete time segments 
5t of the blend interval 2t 0 , the following is performed: (a) 
compute a blended global velocity v of the manipulator as a 
blend of the global target velocity v 0 and global subsequent 
velocity v l9 the blended global velocity v being at least 
approximately equal to the target global velocity v 0 at the 
beginning of the blend time interval and at least approxi- 
mately equal to the global transition velocity v 1 at the end of 
the blend time interval, the blended global velocity includ- 
ing a blended angular velocity co and a blended linear 
velocity v, and then, (b) rotate the manipulator by an 
incremental rotation corresponding to an integration of the 
blended angular velocity to over one discrete time segment 
8t. 


BRIEF DESCRIPTION OF THE DRAWINGS 

FIGS. 1A and IB are graphs showing the blend speed for 
a spectrum of angles (0, 45, 90, 135 and 180 degrees) 
between the initial and final velocities, for the case in which 
the magnitudes of the initial and final velocities are equal 
using linear velocity blending (FIG. 1A) and third order 
polynomial velocity blending (FIG. IB). 

FIGS. 2 A and 2B are graphs comparing linear, third order 
polynomial and cycloidal velocity blends of two orthogonal 
velocities of equal magnitude (FIG. 2A) and two parallel 
velocities of unequal magnitude (FIG. 2B). 

FIGS. 3 A and 3B are graphs showing the spatial paths 
(FIG. 3A) and temporal paths (FIG. 3B) for a transition 
between two orthogonal velocities of equal magnitudes for 
a maximum acceleration magnitude of 10m/s 2 . 

FIG. 4 is a graphical depiction of the velocity blending 
process of Equation 36. 

FIGS. 5 A and 5B are diagrams depicting the spatial 
transition of the target frame (FIG. 5A) and the angular 
velocity vector (FIG. 5B) during an orientation blend using 
the process of Equation 36 with linear blending. 

FIGS. 6 A, 6B and 6C are graphs illustrating the compo- 
nent values of the unit vectors of the frames shown in FIG. 
5A for the n, o and a components, respectively. 

FIG. 7 is a graph depicting the incremental blending of the 
process of Equation 47. 

FIG. 8 is a graph depicting the blending of the process of 
Equation 57 in accordance with the present invention. 
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FIGS. 9A and 9B are diagrams depicting the spatial 
transition of the target frame (FIG. 9A) and the angular 
velocity vector (FIG. 9B) during an orientation blend using 
the process of Equation 57 with linear blending. 

FIGS. 10 A, 10B and 10C are graphs illustrating the 5 
component values of the unit vectors of the frames shown in 
FIG. 9 A for the n, o and a components, respectively. 

FIG. 11 is a simplified schematic block diagram of a robot 
control system employed in carrying out the invention. 

FIG. 12 is a block flow diagram illustrating the blending 
process of the present invention in accordance with a 
preferred embodiment. 


There are several simple choices available for blend 
functions. These are provided below, along with the result- 
ant form of the velocity, acceleration, and blend time. 
Linear Velocity Blending [13] 

m = s (2D 

Vb - v fl (22) 

a= 2t 

l Vb - v fl l (23) 

2 t = — r — 

m2 I max 

Third Order Polynomial Velocity Blending [9, 5] 


DETAILED DESCRIPTION OF THE PREFERRED 

EMBODIMENTS 15 

3 Angular Velocity Blending 
To move smoothly from one segment to another, the 
velocities of the segments must be blended together. To 
achieve this, many strategies employing linear velocity v 20 
have been suggested [9, 13, 5, 14, 6, 7], These techniques are 
discussed below within the framework of the present inven- 
tion. 

The present invention introduces the concept of blending 
angular velocity by blending a global velocity vector v that 25 
includes both an angular velocity vector to and the linear 
velocity vector v. The following discussion utilizes the 
global velocity v of Equation 11 which includes angular 
velocity go with the following convention: 

V a — v ( - 
V b = v i+I 

t - (/«■ - t) 


( 13 ) 

( 14 ) 


f(s) = -2s 3 + 3s 2 

(24) 

(Vb-Va) 

a — (~6s 2 + 65) 

(25) 

lvi,-v 0 l 3 

IsWu 2 

Cycloidal Velocity Blending [7] 

(26) 

f(s) = sin 2 -y s 

(27) 

(Vb — v fl ) 71 . 

a= 2t 2 sums 

(28) 

„ it (29) 

2 

The cycloid has a functional form very close to that of the 
0(3) polynomial, but does not have a discontinuous jerk (the 
derivative of the acceleration). In turn, the 0(3) polynomial 
is superior to the linear form since the latter has discontinu- 
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= Po +v a 2Ts+(v b ~v a )2T f(s) (32) 

where p 0 is the initial position as the blend is entered. The 
form of the integral of the blend function determines the 
spatial form trace by the path. For the three blend functions 


considered, we have: 


Linear: j{s) = s 2 

(33) 

0(3) Polynomial :f(s) = ~-j- s 4 + s 3 

(34) 

s 1 

Cycloidal: f(s) = s * n7W 

(35) 


Equation (33) provides a second order polynomial, and 
the blend is parabolic. Equation (34) provides a fourth order 
polynomial, and the blend that is steeper. (Higher order even 15 
polynomial functions will be increasingly steeper.) The 
cycloidal blend path remains sinusoidal, but has the addition 
of a linear term. 

The graphs of FIG. 3 show the spatial and temporal paths 
for a transition between v a and v b , such that v a J_v fe , lvj=lvj, 20 
with lal max =10 m/s 2 . It is apparent from FIG. 3A that tighter 
cornering can be accomplished with polynomial and cyc- 
loidal bleeding. However, this requires longer blend times 
(or larger acceleration, and therefore greater joint torques 
from the actuators). FIG. 3B shows the positions as a 25 
function of time, which are essentially the integrals of the 
velocities shown in FIG. 3B. The form of these curves also 
represents the functional form of the position blend func- 
tions, Equations (33)-(35). 

5 Blending the Orientation 30 

Blending of the orientation is more complicated than 
position, since the angular velocities are nonholonomic. 
However, this section shows that a close approximation to 
analytic orientation blending can be obtained. This requires 
numeric integration of the rotations obtained from the 35 
instantaneous value of the blended angular velocity. 

5.1 Rotation Matrix Blending for Orientation 

In reference [13] Taylor proposed a method of blending 
orientation based on rotation matrices. A generalization of 
this method will be presented here. In this method, the 40 
amount of rotation contributed by each rotation matrix is 
scaled with the previously presented blend functions: 


“R a °R«=“R a fl R 0 0 Ra (38) 

= W R« °R 0 [co a x]°R a (39) 

=“R a °R a I-m a T]°R„ (40) 

we have: 

“ R(r)=“ Ro °R fl [C0 a 2 ^s-m] a K b [%2tf(j)] (41) 

=“ R a °R a [-GVT] °R a [d) a 2x(i s-ftm a K b [(0 b 2Tf(s)] (42) 
=“R a °R a Uo^is-fisyW)] a R b [go 6 2i tfCs)] (43) 


Further, reference [13] only considers the linear blend 
case with j ! (s)= 1 / 2 S 2 . This gives: 


“RG ^“R* °R a [-co fl T(l-j) 2 ] a R 6 [co^/] (44) 

Substituting Equations (4), (14), and (15) yields: 

“R(f) = (45) 


R a R 


a £ ka, ' 


(T ~(t~t a )) 2 
4t T a 



(T+a-^)) 2 

4t T b 


] 


This is the form of the rotation blend presented in [13]. 
The diagrams of FIG. 5 provide a graphical depiction of 
change in the target frame (FIG. 5 A) and the direction of the 
angular velocity vector (FIG. 5B). (A constant spatial veloc- 
ity has also be used, to spread out the vectors for pictorial 
clarity.) The graphs of FIG. 6 show the change in the target 
frame basis vector components under this transformation. 
5.2 Incremental Rotation Blend Components 

It is informative to look at the rotations that represent the 
individual incremental rotation between successive time 
increments when utilizing Equation (36). Consider the dif- 
ference between successive frames depicted in FIG. 7. 

The incremental rotation between successive orientations 
is: 


“■R(j)=“R 0 °R a [(B fl 2TH(j))) fl R, [to b 2Tf(s)} (36) 45 

=£0 Rc °Ra“Rp (37) 

The graph of FIG. 4 provides a graphical depiction of this 
blending method. Prior to the blend there is motion away 50 
from the orientation of the previous frame, F t _j , and toward 
the intermediate orientation, a=F r . The constant angular 
velocity before the blend is co a , and the blend begins at 
orientation 0 . In this method, for each interval after 0 a 
rotation is construction and applied according to the rotation 55 
matrix blending described by Equation (36) or (37). After 
the normalized blend time s has become unity, the com- 
manded angular velocity will be co^, and the commanded 
orientation is b. After this time, the trajectory continues 
toward the next target frame, F I+1 , at the constant angular 60 
velocity of co t . To avoid faceted motion through the blend, 
the normalized time must be incremented in infinitesimal 
intervals. 

In reference [13], the formulation of this blending scheme 
is presented with respect to frame a, not o. This alternate 65 
representation can be seen by starting with Equation (36), 
and utilizing the identity. 


"'R|3=“'Rp 

(46) 

“Rp- 

(47) 

— 1 “IP - ! °P“ 1 “TP -1 “IP 0 IP “ IP 

— K. p K. a K 0 Ka 1 Kp’ 

(48) 

=“r-V-r-Vr„.“'-Ri3- 

(49) 

"“R~ l p 0 Rp' 

(50) 

» a R-‘p “R^R-'p “e a . “Rp. 

(51) 

=(l+P e p.)+“R^p"e„. a R P ' 

(52) 


(53) 

^RUp+e*] 

(54) 

= p R [ %(s p)Aj+co a (jp) As) 

(55) 
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= p R[to(jp)Ay] (56) 

where e is the infinitesimal rotation operator [4]. This 
result indicates each incremental rotation of Taylor’s scheme 
is equal, to first order, to the rotation provided by the 
instantaneous angular velocity. This implies that it is pos- 
sible to blend the angular velocities utilizing Equation (17), 
and obtain the incremental rotations from the value of the 
instantaneous angular velocity. 

5.3 Angular Velocity Blending for Orientation 

As was discussed in the last section, the incremental 
rotations of an orientation blend may be approximated by 
utilizing the instantaneous angular velocity provided by 
Equation (17). Thus, the orientation of the target frame can 
be computed by utilizing Equations (1), (4), (7), (11), and 
(17): 

m (57) 

= K "^[co^Asi 

n — 0 

- n/N, As - 1 IN 

where N is the total number of steps for the complete blend. 
FIG. 8 provides a graphical depiction of this blending 
method. Before the blend, there is motion away from the 
orientation of the previous frame, F-_ l5 and toward the 
intermediate orientation, a-F-. The constant angular veloc- 
ity before the blend is co a . The blend begins at orientation o. 
For each interval after o, a rotation is constructed and 
applied according to the angular velocity blending provided 
by Equation (17). After the normalized blend time s has 
become unity, the commanded angular velocity will be co fe . 
Ideally, the blend will be complete at the desired orientation, 
o, where the trajectory continues toward the next target 
frame, F I+1 . 

In practice, velocity-based blending can provide equiva- 
lent blends to the rotation matrix method described previ- 
ously. The graphs of FIG. 9 depict the change in the target 
frame (FIG. 9A) and the direction of the angular velocity 
vector (FIG. 9B) for third order polynomial angular velocity 
blending, with lal^^lO m/s 2 . A constant linear velocity is 
also utilized to spread out the origins of the frames for 
clarity. The graphs of FIG. 10 show the change in the target 
frame basis vector components under this transformation. 
Comparing FIGS. 9 and 10 with FIGS. 5 and 6, it is seen that 
there is little difference between blending schemes, even 
when using different blending profiles. 

5.4 Compensation for Second Order Error from Angular 
Velocity Blending 

Looking closely at FIG. 10, it can be seen that there is 
some small residual error in the components of the basis 
vectors. This error results from the second order error 
introduced by the infinitesimal rotation approximation in 
Section 5.2. This can be understood by considering how the 
angular velocity blending effects the rotation blending. Con- 
sider first the case of total completion of rotation by co fl , 
before rotation by co & begins. In this case, the resulting 
rotation is exact: 


°K b =°R a \(o a T} a K b [(o b T:) (58) 



where the rotations ° -R a and * have been divided into N 
parts. Blending the angular velocities is equivalent to chang- 
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ing the order of some of the rotations at the center of this 
chain. For instance, utilizing the infinitesimal rotation 
approximation [4]: 

(l+^Xl+V) B K 2 fc ■ • • ° K N b 
(60) 

(l+V)U+ B O fl R 2 i, (61) 

10 This commutation of the infinitesimal rotations may be 
continued until the proper sequence is attained. However, 
second order errors arise from the initial approximation of 
•R~(l+e) and from the disregard of the commutator (the 
difference between the sequence of the rotations): 

15 

[l+e A , 1 +€ fl ] ~2e A e B ~2e B e A (62) 

The lack of these second order terms explains the small error 
introduced by angular velocity based orientation blending. 
20 The change in position of and a ^b operators in 
the sequence is reminiscent of diffusion. As the 
0 -R a ‘ ‘diffuse’ farther to the right, and the ‘diffuse’ 
farther to the left, the changed in orientation becomes more 
blended. Since the infinitesimal rotations can be represented 
25 by their angular velocity equivalents, the diffusion profile is 
equivalent to the velocity blend profile. For instance, the 
shape of the cycloidal blend profile in FIG. 2B indicates 
more diffusion than the linear one. Further smaller values of 
lal max also imply more diffusion, since they spread out these 
30 curves. More diffusion introduces second order error. There- 
fore, linear blends and high acceleration blends result in less 
residual error for a given value of lal mflx . However, linear 
blends will result in more error if the blend time is fixed 
instead of the acceleration. This can be understood by 
35 lessening the slope of the linear blend line in FIG. 2B, thus 
introducing more diffusion. 

To provide some quantitative description to this discus- 
sion, the following table shows the magnitude of the orien- 
tation error for the example previously considered. 

40 


blend type 

la! max = 10 m/s 2 

lalmax — 5 m/s 2 

linear 

0.29° 

1.16° 

0(3) polynomial 

0.39° 

1.56° 

cycloidal 

0.41° 

1.62° 


It is apparent that these errors are small and may be 
corrected (as described below). Substantially larger errors 
are not possible since they would require much smaller 
50 accelerations which require longer blend times. Too large of 
a blend time multiplied by (£> a or (ti b would indicate a rotation 
greater than 180° in the initial or final legs. Such large 
rotations have been precluded by Equation (5). 

While this small error introduced by one blend does not 
55 necessarily require compensation, the summation of this 
error over successive blends may become significant. To 
compensate for the residual error, we propose the use of a 
correction term which is calculated at the end of every 
velocity based blend of orientation. This term is the incre- 
60 mental rotation from the resultant frame to the desired frame 
at the end of the orientation blend: 


R[*„n cor ]=CK a a K4co.t])- lw K^l] (63) 

In practice, k cor and cor can be easily calculated by 
Equations (5) and (6). A correction velocity may then be 
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calculated and applied to the leg of the trajectory being 
entered, for the time specified to the next via frame: 

®cor^cor caftJto ( 64 ) 

This correction term is modified by a gain, K cor and added 
to the angular velocity (%. (Since it is very small in 
magnitude, concerns about changing the value of co^ have 
been ignored.) The gain is needed to maintain stability in 
what is effectively a low bandwidth feedback controller. If 
Equations (57) and (63) were linear, this discreet time 
controller would be trajectory stable for 0^K cor ^l. How- 
ever, for the nonlinear orientation blending, we have empiri- 
cally found stability for gains of 0^K cor ^03. 

6 Implementation Considerations 

Three main implementational considerations have been 
accommodated in our scheme: maximum acceleration, mini- 
mum blend time, and velocity summation. 

6.1 Maximum Acceleration 

Since the calculated trajectories are to be executed by real 
manipulators, the commanded acceleration must be limited 
to what is achievable. Further, the achievable task space 
acceleration of the arm depends on the configuration of the 
robot arm. In different parts of the workspace, different task 
space accelerations are possible. Therefore, two possibilities 
exist: 1.) limit all task space accelerations to the worst case 
acceleration of the arm, or 2.) create a complete map of the 
achievable task space accelerations, and limit the trajectory 
blending accordingly. However, creating and accessing such 
a map is anticipated to be very cumbersome. Therefore, we 
have currently chosen to work with the first, and simpler, of 
these two options. 

Another consequence of limited acceleration is that it 
erodes the straight line leg segments of the trajectory 
between via frames. For a small enough acceleration, one 
blend will end as another begins. For accelerations smaller 
than this, one blend would have to begin before another 
ends. We do not permit this to occur. In this case, the 
acceleration is increased level of acceleration is not achiev- 
able by the arm, then the via frames are not reasonably 
selected and unavoidable position errors will occur. 

6.2 Minimum Blend Time 

Due to the discrete nature of the computer implementation 
of these algorithms, it is necessary to specify a minimum 
number of iterations over which an acceleration is specified. 
From Equation (b 20) this quantity is the minimum allowed 
value of 2t. If a minimum is not specified, the calculated 
blend time may become comparable to the algorithm cycle 
time. Thus, the calculated velocity and position will be 
discontinuous, providing poor input to the arm controller. 
We have empirically determined and utilized a minimum 
value of twenty iterations per blend. A direct consequence of 
this specification of 2x mr>1 is that the maximum allowed 
acceleration is also limited. If more acceleration is desired, 
and the manipulator is capable of it, then 2x min should be 
reduced. However, to keep the same number of iterations per 
blend with a reduced 2x min , the algorithm rate must be 
increased proportionally. 

6.3 Velocity Summation 

To be able to modify commanded trajectories with other 
control inputs, the commanded variable must be a velocity 
(a generalized flow variable), not a position [11]. FIG. 11 
shows a system for implementing one embodiment of the 
invention. Although not shown in the drawing, the trajectory 
generator optionally may be subject to modification by the 
input of a joystick or a proximity sensor monitor process. In 
FIG. 11, a trajectory generator 10 performs the velocity 
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blending process described above to produce a desired 
sequence of desired end effector frames or positions. These 
are output to a manipulator control system Cartesian con- 
troller 20. The controller 20 computes and outputs a com- 
5 mand angle 0 T to a robot arm controller 30 (in this case, the 
arm controller for the Robotics Research K-1207 Arm). The 
arm controller 30 converts the command angle to motor 
currents and outputs the motor currents to servoes in a robot 
arm 40 (in this case, a Robotics Research K-1207 Arm). The 
10 robot arm 40 returns servo encoder counts to the arm 
controller 30, which computes therefrom and outputs cor- 
responding angle measurements 0 m to a forward kinematics 
processor 50. The forward kinematics processor 50 com- 
j 5 putes and outputs a corresponding measured frame F mcai =x m 
to the trajectory generator 10 and computes and outputs a 
Jacobian transformation matrix J to the manipulator con- 
troller 20. The Cartesian controller 20 and the forward 
kinematics processor 50 perform the foregoing operations 
20 and computations using conventional techniques well 
known in the art. The arm controller 30 and the robotics arm 
40 are commercially available devices. 

Utilizing the velocity blending scheme described in this 
specification with reference to Equation 57, velocity output 
25 is obtained directly. Alternatively, if analytic integration of 
position is used (as in Equation (32)), or if rotation matrix 
orientation blending is used (as in Equation (36)), then the 
velocity must be obtained by differencing the reference 
frames. As will be seen in the next section, this requires extra 
30 computation not needed with a purely velocity based 
scheme. 

7 Computational Costs 

Table I provides an outline of the computational steps and 
35 costs for both position-based and velocity-based blending. 
The equations involved in each step are also summarized. 
Finally, an estimate of the computational complexity is 
given by stating the number of additions, subtractions, 
multiplies, and divides required, as well as the trigonometric 
40 (and square root) operations needed. 

Under the operations column, the values are the number 
of standard math operations (+-*/) and the number of 
trigonometric and other math operations (e.g., sin, cos, sqrt, 
and so forth). The top section of the table reviews some 
45 common steps needed for both schemes. Of these, the frame 
differencing and frame incrementing are very costly. The 
calculation of ;f (s) or f (s) is variable since it depends on the 
blend functions chosen. 

The second and third sections of the table show the 
50 algorithmic differences between the position/orientation 
blending and the velocity blending methods. The most 
striking difference between the two formulations is the 
reduced computational cost of the velocity blending method. 
During a blend it requires only 12 operations, while the 
55 position/orientation method requires 263 operations plus 
eight costly trig or square root calls. The situation is much 
the same during the straight line leg segments of the trajec- 
tory, where the velocity based scheme requires zero opera- 
tions, while a completely position based scheme requires 
60 160 plus 5. The efficiency of the velocity based scheme is 
paid for by the overhead necessary during the transition 
from blend to leg segments. At this juncture, the velocity 
scheme must make 207 plus 6 operations, while the position/ 
orientation scheme requires only 69 plus 2. However, this 
65 overhead occurs only once for each via frame, compared to 
the hundred or thousands of iterations that occur for the 
blend or leg segment 
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TABLE I 


Algorithm Step 

Eqns 

Ops 


Common 



5 

vAt = framedif(F 1 ,F 2 ) = D() 

1-6, 11 

69, 2 


F 2 = frameincCF^vAt) = I() 

1-4, 7-11 

91, 3 


calc f(s) or f (s) 

17, 32, 36 

variable 


v 2 = vecscaleCv^func) = S() 

11 

7, 0 


Position/Orientation Blending 

20 

Method 

variable 

10 

blend 

calc f(s) 

33, 35, 34 

variable 


v a = S(v a ,s - f(s)) 

36 

6 

15 

Vj 3 = S(v b ,f(s)) 

36 

6 

F a = I(F 0 ,v a At) 

36 

91, 3 


F b = I(F a ,VpAt 

36 

91, 3 


v = D(F c ,F b )/At ob 
leg 

1-6, 11 

69, 2 


F(t) = mApmmMm 

1-4, 7-11 

91, 3 

20 

v = D(F(t),F(t - At))/At 
transition 

1-6, 11 

69, 2 


v b = D(F i5 F i+1) /T i+1 

1-6, 11 

69, 2 


a < lal max , X > 

Velocity Blending Method 

20 

variable 

25 

blend 

calc f (s) 

21, 27, 24 

variable 


v - S(v a ,l - f(s)) 

17 

6 


v + = S(v b ,f(s)) 
leg 

nothing, constant v = v a 
transition 

17 

6 

0, 0 

30 

v b = l-KFiTi + i)/T i+1 

1-6, 11 

69, 2 

35 

F' b = KF^v^) 

63, 64 

69, 2 

v b + = D(F b ,F' b )/T i+1 - Xj) 

63, 64 

69, 2 


a < la! max , x > x^ 

20 

variable 



computations. Obviously, velocity blending introduces a 
significant computational savings. 40 

It is important to note that some of the computational 
advantage of velocity blending is introduced by the assump- 
tion that the output of a trajectory generator must be a 
velocity. The position/orientation scheme must utilize a 
velocity calculation step during the blend and leg segments 45 
which costs 69 plus 2 operations. However, even without 
this step the velocity blending method is significantly faster. 
Further, it was shown in the last section why velocity output 
is more useful. 

One other computational burden is introduced to the 50 
position/orientation method by the assumption that position, 
[p,k(j),\j/], is specified as a function of time during the leg 
segment. Alternatively, the leg segment velocity could be 
precomputed and utilized directly as in the velocity blend 
method. Since k is constant during the leg segment, no errors 55 
would be introduced. Also, the leg velocity must be com- 
puted anyway if the maximum acceleration checks are to be 
performed (as is assumed). 

8 Implementation 

We have implemented this algorithm on an SGI Iris 60 
workstation for simulation, and on a VME based 68020 
microprocessor for control of 7 DOF Robotics Research 
K-1207 Arm. The end-effector of the robot arm carries an 
array of sensors: two CCD cameras, two proximity sensors, 
an optical pyrometer, a gas sensor, and a force sensor. The 65 
addition of eddy-current and contact acoustic sensors are 
planned. While our frame to frame motions are designed to 
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aid inspection by these devices, the presented technique is 
obviously extensible to motion required for purposes other 
than inspection. 

8.1 Experimentation 

The blending algorithm has also been implemented for 
real-time control on a 12.5 MHz Heurikon 68020 processor. 
For the tests, a trajectory similar to the simulation trajectory 
has been executed. However, since the robot base position is 
fixed, the size of the inspection area is restricted. A total of 
twelve via frames are used to scan a rectangular shape about 
half as large as that in the simulation. Linear blending was 
arbitrarily chosen for these tests. During experiments the 
minimum time between frames is 3 seconds. The real-time 
process runs at 44 Hz, or =22.7 ms, giving approximately 
132 iterations for each frame to frame motion. (The control 
rate is governed by other control software, not the process- 
ing requirements of the trajectory blending algorithm, which 
we have shown to be quite minimal.) The position gain was 
K p =20, and the trajectory correction gain was K cor =0.3. The 
minimum blend time was 2x min -20 iterations, or about a half 
second. The maximum acceleration was lal max =10 m/s 2 . 

9 Angular Velocity Blending Processing Description 

The angular velocity blending method described above is 
now described in greater detail with reference to the steps 
depicted in FIG. 12. 

The process begins by initializing key parameters (block 
100 of FIG. 12 ), by setting the index n to one, setting the 
current global velocity v an the initial velocity v 0 to zero, 
while setting the current frame F to the measured frame of 
reference F meas and setting the current time t to the minimum 
blend time x min . The previous frame corresponds to F / _ 1 of 
the graph of FIG. 8. 

Typically, the user specifies the next target frame F 0 
corresponding to F t - or point a of FIG. 8, the subsequent 
frame F 1 corresponding to F I+1 of FIG. 8. As a slight 
departure from the notation employed in FIG. 8, the process 
illustrated in FIG. 12 employs the index n to keep track of 
the successive frames, and the next target frame F 0 is set to 
F n-1 while the subsequent frame F x is set to F n . 

The description of this process will now skip to a point at 
which blending has been completed for a current frame, so 
that the index n is to be incremented by one: n=n+l. This 
incrementing of the index n is performed as part of an 
increment step of block 85 , which begins a new iteration of 
the cyclic process. In the increment step of block 85 , the 
current time is shifted by T 0 , the current target frame F 0 is 
updated to the subsequent frame F l of the previous iteration 
and the current target frame F x is set to the next frame F n 
specified by the user. The global velocity v includes both the 
linear velocity v and the angular velocity to, in accordance 
with Equation 11. In the increment step of block 85 , the 
initial block velocity v 0 is corrected by an error correction 
global velocity v e computed in an other part of the process 
in accordance with Equation 65 in a manner described below 
herein. As will be described, the purpose of this correction 
is to compensate for a residual error at point b of FIG. 8 
corresponding to a blend exit frame F' 0 , specifically the 
residual error discussed with reference to Equation 63. 

Next, a differentiation step of block 90 is performed using 
a computation described below called framedif to compute 
a global velocity necessary to move from frame F 0 to 
frame F 2 within a time T l specified by the user. The step of 
block 90 then computes from the two global velocities v 2 
and v 0 , and from a maximum acceleration specified by the 
user, a blend interval time T 0 in accordance with Equation 
23, 26 or 29 using a process calctau described later in this 
specification. 
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At a decision block 120 , a determination is made whether 
the blend time t=T 0 -T o has been reached. If not (YES branch 
of block 120 ), then the current global velocity v is kept 
constant at v 0 (block 125 ) and the time t is incremented by 
adding to it a time differential 6t (block 130 ). 5 

A frame incrementing step 135 is performed using the 
current angular velocity co of the current global velocity v. 
This frame incrementing step 135 is a rotation of the 
manipulator though an incremental angle equal to coxSt. The 
frame incrementing step of block 135 updates the current 10 
frame F. The process then cycles back to the determination 
step of block 120 and continues in a cycle constituting the 
steps of blocks 120 , 125, 130 and 135 . This cycle is repeated 
until the time t reaches the blend time t=T 0 -T 0 (NO branch 
of block 120 ). 15 

Once the blend time has been reached, a determination is 
made at a decision block 140 whether the current time t falls 
within the blend time window T 0 -T 0 <t^r 0 +T 0 . If so (YES 
branch of block 140 ), then the blend function ^f'(t,s) is 
computed (block 145 ) according to Equation 33, 34 or 35 20 
and this function is used to update the current blended global 
velocity v using Equation 16. The current time is incre- 
mented in the step of block 150 as in the step of block 130 . 
The frame incrementing step of block 135 is performed, but 
this time using the current angular velocity co of the global 25 
velocity v blended between v 0 and Vj in accordance with 
Equation 16 by the step of block 145 . The process cycles 
back to the determination step of block 140 . A cycle con- 
stituting blocks 140 , 145 , 150 and 135 is repeated until the 
time t exceeds the blend period (NO branch of block 140 ). 30 
Each iteration of this cycle produces an incremental rotation 
of the frame using an angular velocity vector co updated each 
iteration. 

Upon completion of this cycle (i.e., when t>T 0 +r 0 ), a 
sequence of incremental rotations has been performed as 35 
depicted in FIG. 8 to blend the manipulator motion from the 
initial frame F 0 to the blend exit frame F' 0 . With each 
iteration of the process after each time increment 8t, the 
frame increment step of block 135 computes an updated 
frame F, which is output by the trajectory generator 10 of 40 
FIG. 11 to the manipulator control system cartesian control- 
ler 20 of FIG. 11 to produce a command 0 r to the robot 
servos to rotate and/or translate the robot manipulator to the 
updated frame. 

Taking the NO branch of block 140 , the current frame F 45 
is compared to the desired blend exit frame F' 0 (correspond- 
ing to point b of FIG. 8) obtained from a frame incrementing 
step 105 . The frame incrementing step is an incremental 
rotation through an angle obtained by multiplying the initial 
angular velocity co 0 by half the blend time, t 0 , obtained from 50 
the step of block 90 . (Both frame incrementing steps 105 and 
135 employ a process called frameinc which is described 
later in this specification.) 

Then, a differentiation step 110 computes a velocity error 
correction v e by dividing the difference between the current 55 
frame F and the desired blend exit frame F' 0 by the time 
remaining to the next frame, T 0 -t 0 . The incrementing step 
of block 85 is repeated, and the entire process begins the 
next iteration with a new target frame FfH-2* The adding step 
v o“ v o v e of block 85 compensates for the residual error of the 60 
previous blend cycle and implements the correction of 
Equation 65 . (The differentiation steps of both blocks 90 and 
110 employ a process called framedif defined later in this 
specification.) 

The foregoing process is now set forth in greater detail as 65 
a series of program language statements, each statement 
being accompanied by an explanatory remark in italics. In 
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the following, there is a main program, called main body 
which calls for four different sub-routines named, respec- 
tively, frameinc, framedif, calctau and calcfprime. 

9.1 Main Body 


BEGIN: 

n — 0 initialize counter 
F = F mcas initialize desired to current frame 
F 0 = { Ko.Po.YoTol next target frame 
v = 0 start at rest 
v 0 = 0 initial velocity is zero 
v c = 0 initial error correction velocity is zero 
t = Tjnin initialize time 
NEXT: 

v 0 = v 0 + v c modify target velocity 
n = n + 1 increment counter 

F x = F n = { R n ,p n ,V n ,T n } subsequent frame (if unavailable 
set to F n _j) 

V, = framedif (F 0 ,Fi )FT 1 determine average velocity 

needed between frames 

Tq = calctaufVi - v 0 ) calculate and shift blend time 
while(t < T 0 + t 0 ){ 
t = t + 8t increment time 
LEG: 

if(t < T 0 - t 0 ) v = v 0 constant velocity in leg 
BLEND: 

if (To ~ Tq < t < Tq + t 0 ){ 

f = calcfprime((t - T 0 + t 0 )/2i: 0 ) get blend function 
value 

v = v 0 (l — f) + v x f calculate blended velocity 
} 

F = frameinc(F,v5t) increment the desired frame 
} 

if(t £ T 0 + T 0 ){ 

F'o = ffameinc(F 0 ,v 0 To) determine correct frame position 
for blend exit 

v c = framedif(F,F' 0 )/(T 0 - t 0 ) determine additional velocity 
needed 

t = t - T 0 shift time 
F 0 = Fj shift frames 

v 0 = Vj shift velocity value 

goto NEXT: repeat the process 

} 


9.2 Frame Differencing Subroutine 


framedif(F 0 ,F 1 ) frame differencing subroutine 
{ 

v = (Pi — p 0 ) linear velocity assuming unit time 
ksin<|) = -^- (no X m + oo x oi + ao x ai) 

coscj)=:-~- (no • ni + oq* oi +ao • ai - 1) 

if(lk sin(J)l = 0) { ambiguous result 

if(cos(j> = 1) <j> = 0 no difference in frames 
if(cos<{) = —1) { greatest difference in frames 

<{> = 71 

k I = N| fe+l)/2 

if = 0, substitute o*,Oj,,o z 

ky = n^kc if k*(oJ = 0, substitute a*, 

K = n*/2kx 

> 

} 

if(!k sin(f)l ^ 0) <J) = tan'Tsintj), cost})) minimum angle 
between frames 
ksin<j) 

05 ~ lksin<|>l ^ 

angular velocity assuming unit time 

y = (Vi- Vo) scalar velocity assuming unit time 
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-continued 


18 

9.5 Calculation of Blend Profile Subroutine 


retum( [v, to, \|/]) return frame difference 

} 


5 

9.3 Frame Incrementing Subroutine 


frameinc(F 0 .v 0 t 0 ) frame incrementing subroutine 
{ 

<j> = lG0ot o l rotation angle 
k = C0 o to/<J> rotation axis 
S 0 = sin([> 

C* = cos(J) 

V 0 = 1 - cos<|> 


+ Q 


kjky V (j) k z S$ kjkjVij) + kyS^ 


R = 


kxkvV* + k,S* 


kykyVtt + Q> 


k y k z V $ — kjSij) 


^ kik^y^ k v Sb kykjV^ + kjS(*j k z k z V 5 + 

rotation matrix 

Ki = K Ro increment orientation 
Pi = Po + Vcta increment position 
Vi - Vo + Yok) increment scalar 
retumtFj = { RuPuYiTo + to} ) return the new 
frame 


} 


9.4 Calculation of Blend Time Subroutine 


10 


15 


20 


25 


calctau(Av) 

{ 

if(LINEAR) retumCAv^lal^) 


linear blending 

if(03P0LY) return (3Av/4lal max ) 

third order 

polynomial blending 


if(CYCLOID) retum(jrAv/4lal max ; 

) cycloidal 

blending 

} 



calcfprime(s) s is normalized time 
{ 

if(LINEAR) retum(s) linear blending 
if(03P0LY) retum(-2s 3 + 3s 2 ) third order 
polynomial blending 

if (CYCLOID) return f sin 2 -j- s ^ cycloidal blending 


10 Conclusion 

This specification has presented a new formulation of 
trajectory generated based on velocity blending. First, a new 
formulation for trajectory blending was provided, allowing 
for the direct comparison and utilization of numerous blend 
functions. Then, a generalized version of the previously 
proposed orientation matrix blending formulation was 
reviewed. It was shown how a first order approximation of 
this scheme leads directly to angular velocity blending for 
orientation change. Further, the residual error incurred was 
explained, quantized, and compensated. Also explained 
were implementational considerations such as acceleration 
limits, velocity summation requirements, algorithm compu- 
tation rates and complexity. Finally, the results of imple- 
mentation of this scheme in both simulation and real-time 
experimentation were graphically presented. Both the analy- 
sis and implementation has shown that the speed and sim- 
plicity of the velocity-blending formulation enable its ease 
of use for real-time manipulator trajectory generation. 

Appendix A contains a listing of a C-language computer 
code employed in carrying out the invention. Each of the key 
statements in the listing is accompanied by an explanatory 
remark in italics. 
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11 APPENDIX A: C-Code Listings 
11.1 Via.c 

5 /* viajsocket.c: trajectory generator reading points form 

socket 

queue * 

10 

#include<stdio.h> 

#incl ude< sign alh> 

#include< math.h> 

15 #include<cmu.h> 

#incl ude< strings . h > 

#include< macros . h > 

# include” via.h” 

20 

Frame-t Via[] = { 

{ 

{ 1 . 0 , 0 . 0 , 0 . 0 ], 

25 { 0.0, 1.0, 0.0}, 

{ 0 . 0 , 0 . 0 , 1 . 0 }, 

{-1.0, 1.0, 1.0} , -HALFPI, HALFPI, 1.0} 

,{ 

30 { 0.0, 1.0, 0.0}, 

{- 1 . 0 , 0 . 0 , 0 . 0 }, 

{ 0 . 0 , 0 . 0 , 1 . 0 }, 

{ 0.0, 0.0, 0.0}, -HALFPI, HALFPI, 1.0} 

{ 0 . 0 , 0 . 0 , - 1 . 0 }, 


35 
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{- 1 . 0 , 0 . 0 , 0 . 0 }, 

{ 0 . 0 , 1 . 0 , 0 . 0 }, 

{ 1.0, 1.0, 1.0}, -HALFPI, HALFPI, 1.0} 

5 }; 

#define VIAPTS ( int )( sizeo f( Via) /sizeof( Framed ) ) 
10 char server. hostname[80] — ’’loren”; 

FILE * tfp, *pfp, *vfp, *mfp, *ffp; 

15 hit Traj -Running = 1; 


/* signal handler*/ 
encltrajQ 

{ 

TrapRunning = 0; 

} 


/% :fc ^ ^ ;jc ^ 5«c sj< ^ ^ if: if: if: if: if: %./ 

main() 

{ 

char buf [80] ; 
char prompt [80]; 
char arts [80]; 
int clone = 0: 
int child: 
int vianum = 0; 


35 
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double basetime = 0.0; 

getstr( "Hostname of server: ", server Jiostname . 
5 seiner Jiostname ) ; 


/* parent*/ 
if( !(child=fork()) ){ 
socket-server Jnit ( SOCKPORT ) : 
while(strncmp(getstr( " send via frames?". 
"y",buf),"y",l)==0){ 
vianum = 0; 

while(vianum < VIAPTS){ 
PERROR(socket_writ.e( &,( Via[vianum+4-] ) . 
sizeof( Framed ) ) ) ; } 

priiitf("done sending via frames. . . "); 

} 

ldll( child, SIGUSR2): 
exit(0): 

} 

/* child*/ 

else{ 

signal ( S I GU S R2 .endtraj ) ; 
sleep(2); 


/*open files for data logging *j 
pfp = fopen("p.dat", "w"); 
vfp = fopen("v.dat", "w"); 
mfp = fopen("m. dat". "w"); 
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tfp = fopen("t . dat". "w"); 
ffp = fopen("f .dat", "w"); 

printf("pfp = 0x°/„x, vfp = 0x°/„x, mfp = 0x%x, tfp = 
5 Ox°/ 0 x, f fp= Ox%x\n" , pfp, vfp, mfp, tfp, ffp) 

socket_client_init(seiwJiostname,SOCKPORT); 
socket_noblock(); 
t raj gen (); 

10 

/* close data files*/ 

{int. i; 

for(i= VIAPTS— 1 :i>0;i — ) 

15 fprintf(pfp, "%f %f °/of\n", Via[i].p[0], Via[i].p[l], 

Via[i].p[2]); } 

fclose(pfp); 
fclose(vfp); 

20 fclose(mfp); 

fclose(tfp); 
fclose(ffp); 

} 

25 } 


trajgenQ 

{ 

VeLt v. fdel; 

Frame.t via[2]; 

Frame.t f, fnew; 

static int profile = CYCLOIDAL; 
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static int vfn, vfa, vfb; 
static int inleg = FALSE; 
static int atstart — TRUE; 

5 static double t = 0.0; 
static double ta. = 0.0; 
static double tb = 0.0; 
static double tau. twotau; 

0 static VeLt va = ZEROVEL.T; 

static VeLt vb = ZEROVEL_T; 

static VeLt vbminusva = ZEROVEL.T; 
static VeLt verror = ZEROVEL.T; 

? static VeLt void = ZEROVEL.T; 

register double cs, ss; 
register double temp; 
register double s; 

VeLt vtemp; 
double vmag; 
int i: 

int j = 0: 

5 

static double basetime = 0.0; 

/* needed only for printing nice graphs */ 


30 


f = Via[2]; 


yL ^ ^ ^ ^ ^ 4 s ^ ^ 

/*main loop*/ 

wliile(Traj .Running) { 
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vfn = 0; 
vfa = 1; 

5 . vfb = 0; 

viafvfb] = f; 
ta = 0.0; 

tb = 0.0; 

0 inleg = 0; 


/* needed only for data logging */ 

fprintf(pfp, "%f °/ 0 f %f\n". via[0].p[0], via[0].p[l], 

15 ™[0].p[2]); 

socket_normal(); /*cause read to block on empty queue*/ 
atstart = TRUE; 

20 goto START; j *when starting traj, need to get next 

frame, 

t am/ 


25 


30 


/* trajectory generating time loop */ 

/* ( start t obtained from tau, at bottom ) */ 

for(; t < tb+tau; t += TINC){ 


35 


s = (t — (ta — tau)) / twot.au; 


y4fc # 5fc ^ 
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/* if in blend */ 

^ ^ ^ ^ * vj^ 

if(s>0.0 kk s<1.0 kk vfii>0){ 
inleg = FALSE; 
switch (profile)! 
case CYCLOIDAL: 
ss = sin(HALFPI*s); 

VE LS C AL E ( vbminusva,ss* ss , vtcmp); 

VELADD(va,vternp,v); 

break; 

case LINEAR: 

VELS C' AL E(' vbminusva,s, vtemp ) ; 

V E L A D D ( va , v t emp , v) ; 
break; 

} 

} 

/* if in leg */ 
else{ 

/* if first step of leg */ 
if(!inleg){ 
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/* add in integration error correction term */ 
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VELSCALE( vb,t — TINC- ta.vtemp ); 

/*not exactly tau, but close*/ 

FRAMEIN C( via[vfa] ,vtemp,fnew) ; 

5 F R AMEDI F (fnew,f, vtemp ) ; 

VELSC ALE( vtemp, 1.0/ viajvfb] .t, ,verror) ; 
VZERO(verror.v) ; 

/* trans. integration works well*/ 

10 

j ^ ^ yjs >jc ^ * % j 

/* inchworm values */ 

/jC 5|C5|c 5*C 5ji s)c 5$< sfc 5{C >j</ 

15 START: inleg = TRUE; 

va — vb; 
ta = tb; 
vfa — vfb; 

20 

/%. s(c sf; ^ ^ ^ sjc ^ 3 $; s(; 5^ %/ 

j* is there another via point? */ 

/% ;jc 5|< 5}s ^ 5|c if; sf: % sf: ^c>)s ;{; sj? sfc sfs 5}i ^ $£ %./ 

25 

if(socket_iead(&via[(vfn+l) & 1], sizeof(Frame_t)) < 
0){ VELZERO(vb): 

} 

else{ 

/* frame has been read from socket*/ 
socket jioblockQ; 

j*cause read not to block on empty queue*/ 
vfb = ( H — hvfn) V 1; 

F RAM EDI F ( via[vfb] . viajvfa]. vtemp) ; 

VELSC ALE( vtemp. 1.0/via[vfb].t, vtemp); 


35 
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VEL AD D (■ verror,vtemp ,vb) ; 
tb += via[vfb].t: 

} 

5 

/* get tau; check bounds */ 

10 VELDIF( vb.va.vbminusva.) ; 

VEL M AXM AG( vbminusva, temp ) ; 
switch (profile)! 
case CYCLOIDAL: 

15 twotau = HALFPI*temp/AMAX; break; 

case LINEAR: 

twotau =temp/AMAX; break; 

} 

20 

/* check min/max of tau */ 

25 if( twotau < TWOTAUMIN) twotau = TWOTAUMIN; 

else if(twotau > via[vfa].t){ 
twotau = via[vfa].t; 

printf("TauA > 0.5 tA: Will attempt to exceed 
30 AMAX.\n") ; } else if( twotau > via[vfb].t){ 

twotau — via[vfb].t; 

printf("TauB > 0.5 tB: Will attemptto exceed 
AMAX.\n") ; } tau - 0.5* two tau; 

35 if(atstart){ 

atstart = FALSE; 
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basetime += t+tau; /* needed only for data 

logging*! 

t. = — 1.0*tau; /*set effective starttime of 

5 loop*/ } 

}/*end if(Hnleg)*/ 


} 


15 


20 


25 


/% sfc s(c sfc :{c ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ ^ *j 

/* get next pos; trapezoid rule */ 

/* * >}: =Jc * * * * s|e ^ ^ 'Jf ^ * He * * * =t= * *j 

VELADD(v,volcl.fdd): 

VELSCALE(fdel. 0.5*TINC, fdel); 

F R AM El N C ( f .fdel,fnew ) ; 


fncw.t = t; 
f = fnew; 


void = v; 


j'Jf >(< :)< j|c % ^ ^ ^ ^ ^ ^ * =1= * *j 

/*needed only for data logging*/ 
fprintf(pfp, "7 0 f 7,f 7„f\n", f.p[0] . f.p[l] . f.p[2] ) ; 
j* f print f(pfp, ”%f %f\n”, f.p[0], fp[l]); */ 

VMAG(v.\v,vmag); fpriiit,f(mfp, "7,f %f \n". t+basetime, 
vmag); if ( j H — h > VINC){ 


/*print noa frame vectors to file*/ 
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fprintf(ffp, " If If If If If If \n", f.p[0], f.p[l] : 
f.p[2], f.n[0],f.n[l],f.n[2]); 

fprintf(ffp, "°/«f %f If °/,f If If \n", f.p[0]. f.p[l], 
^ f.p[‘2], f.o[0],f.o[l].f.o[2]); 

fprintf(ffp, "7«f °/,f %f %f °/„f °/ 0 f \n", f.p[0], f.p[l], 
f.p[2], f.a[0].f.a[l],f.a[2]); 

' /*print angular vel vecs to file*/ 

fprintf(vfp, "7 0 f 7of 7of 7«f 7.f 7,1 \n", f.p[0], f.p[l], 
f.p[2], v.w[0], v.w[l], v.w[2]); 


} 

} 


35 
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11.2 Via.h 

/* via.h: trajectory generator include hie 
* 

5 */ 


^define TINC 0.01 
10 #definc TINCINV 100 
#define VINC 10 

^define AM AX 10.0 
#defineTBUF (100.0*TINC) 

15 # define TWOTAUMIK (20.0*TINC) 
#define MAXVIAFRAMES 10 


#define SOCKPORT 40011 

#define CYCLOIDAL 0 

# define LINEAR 1 

tvpedef struct { 
double v [3] ; 
double w[3]; 
double psidot; 
double chidot; 

} VeLt; 

tvpedef struct { 
double n[3]; 
double o[3] ; 
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double a[3]; 
double p[3]; 

double psi; /*arm angle*/ 

5 double chi; /* elbow angle*/ 
double t; 

} Frame _t; 

10 # define ZEROVEL.T {{0. : 0.,0.},{0.,0,0.},0.,0.} 

#define VELADD(.AA,_BB,.CC){\ 
VADD((_AA).v, (JBB).v. (_CC).v);\ 

15 VADD((_AA).w, (_BB).w, (_CC).w);\ 

(-CC).psidot = (_AA).psidot + (_BB).psidot;\ 
(-CC).chidot = (_AA).chidot + (_BB).chidot;\ 

} 

20 

^define VELDIF(_AA,JBB,_CC){\ 
VDIF((_AA).v, (_BB).v, (_CC).v);\ 
VDIF((_AA).w, (JBB).w, (_CC).w);\ 
(-CC).psidot = (_AA).psidot — (_BB).psidot;\ 
(_CC).chiclot = (-AA).chidot. — (_BB).chiclot:\ 

} 

30 

#define VELSCALE(_AA,_BB,.CC){\ 
register double _DD;\ 

_DD = JBB;\ 

VSCAL AR( ( _AA) . v, _DD, (_CC).v);\ 
VSCALAR((_AA).w, _DD, (_CC).w);\ 
(_CC).psidot. = (_AA).psiclot * -DD;\ 
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(_CC). chidot = (_AA). chidot * _DD;\ 

} 

5 #define VELCOPY(_AA,_BB){\ 
VCOPY((_AA).v, (_BB).v);\ 
VCOPY((_AA).w, (_BB).w);\ 
(-BB).psidot. = (_AA).psidot;\ 

10 ( _BB) .chidot = ( _AA).chidot:\ 

} 

#define VELZERO(_AA){\ 

15 YZERO( ( _A A) . v) : \ 

VZERO ( ( _A A ) . w) ; \ 

(-AA).psidot. = 0.0;\ 

(_AA). chidot. = 0.0;\ 


# define VELM AXM AG ( „AA._BB) { \ 
register double _CC;\ 

VMAG(( JVA).v, _BB);\ 

VMAG((_AA).w, _CC);\ 

if( (_BB) < (_CC) ) (JBB) = (_CC);\ 

if( (-BB) < (_AA).psidot) (_BB) = (_AA).psidot;\ 

if( (_BB) < (_AA),cliidot) (_BB) = (.AA). cliidot ;\ 

} 

# define VELPR1NT(_AA){\ 

VPRINT ( ( _AA) . v) :\ 

VPRINT((_AA).w);\ 
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R P RINT( ( _A A) .psidot, ) ; \ 

RP RINT ( ( _A A ) . chi dot ) ; \ 

} 


/ xL* O- vly* vL* -»X» «a + xL* si" vL» vl-» ’d/ O/ *1* %L* vl* *4^ 'X 'I' 4' vlv- *1# %!• \ 

f ^ vp* 2 js ?fv ?p, >p% vp* Jp* /fv ']'* 'j' Pp» '’p 'f' *7% 'p* *7* <7* *7* -x> 7> *p» *p* -p 'p «T" *T** *T* "T* \ 

* * 

* k*sin(phi) = -(nd x n + od x o + ad x a) / 2.0 * 

* cos(phi) = ( (nd . n + od . o + ad . a) / 2.0) - 0.5 * 

* * 

* _AA is final (desired) Framed * 

* _BB is initial (measured) Framed * 

* _CC is tlie velocity vector VeLt * 

* * 

v|^ v|^ ^ j|v i|c ^ ^fc 

#define FRAMEDIF( .AA, _BB, _CC ){\ 
register double _DD[3], _EE[3], _FF[3];\ 
register double .SPHI, _CPHI, _MAG;\ , 
register double _GG, _HH, JI;\ 

VDIF( (_AA).p. (_BB).p, (-CC).v );\ 

(_CC). psidot = (_AA).psi — (_BB).psi;\ 

(_CC).chidot = (_AA).chi - (_BB).clii;\ 

VCR0SS( (_AA).n. (_BB).n, _DD );\ 

VCROSS( (_AA).o, (-BB).o, _EE );\ 

VCROSS( (_AA).a, (JBB).a, _FF );\ 

VADD3( _DD, _EE, _FF, (_CC).w );\ 

VSCALAR( (-CC).w. -0.5, (_CC).w );\ 

VMAG( (-CC).w, _SPHI);\ 
if(fabs(_SPHI) > EPSILON){\ 
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VDOT( (_AA).n. (JBB).n, _GG );\ 

VDOT( (-AA).o, (_BB).o, _HH );\ 

VDOT( (_AA).a, (_BB).a, _II );\ 

5 _CPHI = (_GG + _HH + JI) * 0.5 - 0.5;\ 
.MAG = atan2( JSPHI, _CPHI ) / _SPHI;\ 
}\ 

else .MAG = 0.0;\ 

10 VSCALAR.( (_CC).w, .MAG. (_CC).w ):\ 

} 


15 #define FRAMEINC ( _AA, BB. _CC){\ 
register double _RR[3][3]. _KK[3];\ 
register double .PHI. _SPHL _CPHI, _VPHI;\ 

VADD( (-AA).p, (JBB).v, (-CC).p );\ 

20 (-CC).psi = (_BB).psidot + (.AA).psi;\ 

(_CC).chi = (_BB).chidot 4- (_AA).chi;\ 

VMAG( (-BB).w, .PHI );\ 

if(fabs(.PHI) > EPSILON) {VSCALE( (_BB).w, 1.0/.PHI, 
25 -KK);}\ 

else {VZERO(_KK);}\ 

.SPHI = sin(_PHI);\ 

-CPHI = cos(_PHI);\ 

-VPHI = 1.0 - _CPHI;\ 

_R.R[0][0] = _KK[0] * _KK[0] * .VPHI + _CPHI;\ 

_RR[1][1] = _I\K[1] * _KK[1] * .VPHI + _CPHI;\ 

_RR[2][2] = _KK[2] * _KK[2] * .VPHI + _CPHI;\ 

_RR[1][0] = _KK[0] * _KK[1] * .VPHI + _KK[2] * _SPHI;\ 
_R.R[0][1] = _KK[0] * _KK[1] * .VPHI - _KK[2] * _SPHI;\ 
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_RR[2][0] = _KI\[2] * _KK[0] * _VPHI - _KK[1] * JSPHI:\ 

_RR[0][2] = _KK[2] * _KK[0] * .VPHI + _KK[1] * _SPHI;\ 

_RR[2][1] = _KK[1] * _KI<[2] * .VPHI + _KK[0] * _SPHI;\ 

s _RR[1][2] = _KI<[1] * _KI<[2] * .VPHI - _KI\[0] * _SPHI;\ 

VROT( (j\A).n, -RR, (_CC).n );\ 

VROT( (_AA).o, .RR, (-CC).o );\ 

VROT( (-AA).a, _RR, (_CC).a );\ 

10 } 


#clefine F RAMEPRI NT ( _A A) { \ 
15 VPRINT((AA).n);\ 

VP RI NT ( ( .A A ) , o ) ; \ 
VPRINT((.AA).a);\ 
VPRINT((_AA).p);\ 

20 RPRI NT{ ( _A A ) . psi ) ; \ 

RPRIKT((j\A).chi);\ 
RPRINT((_AA).t.):\ 

} 

25 

30 


35 
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il. 3 Macros.h 

/*some useful macros*/ 

5 /*some useful defines */ 

#ifndef PI 

#clefine PI 3.1415926535897931 
#endif 

10 #definc INVPI 0.318309886 
#ifndef HALFPI 

#definc HALFPI 1.5707963267948965 
15 #endif 

#ifndef TWOPI 

# define TWOPI 6.2831853071795862 
T^endif 

20 

#define EPSILON l.c-5 
#define LARGEREAL I.elO 

25 

#ifndef TRUE 
^define TRUE 1 
#endif 

#ifndef FALSE 

30 

#define FALSE 0 
#endif 


35 


#dcfine ABS(x) (((x) > 0) ? (x) : (-(x)» 
#define SGN(x) (((x) == 0) ? 0 : (x) / ABS(x)) 
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# define DPRINT(message) { \ 

printf(" (f ile %s, line %d) ", __FILE__, __LINE__); \ 

printf(": %s\n". message); \ 

} 

^define PERROR(rontinc_call){ \ 
if((rout.ine_call) < 0){ \ 

p error ( "ERROR: 1 routine_call 1 \n"); \ 

exit(— 1): \ 

} \ 

} 

#define VXERROR(routine_call){ \ 

if((routine_call) < 0){ \ 

printf( "ERROR: 1 routine_call 1 \n"); \ 

exit(— 1): \ 

} \ 

} 


25 / / 

tvpedef double Vcc3_t[3] ; 

/* vector operations */ 

# define VCROSS(_A,JB,-C){\ 

_C[0] = _A[1]*J3[2] - _A[2]*_B[1]; \ 

_C[1] = _A[2]*_B[0] - _A[0]*_B[2]; \ 

_C[2] = _A[0]*_B[1] - _A[1]*_B[0]; \ 
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} 

#define VDOT(_A._B,.C){\ 
s _C = -A[0]*JB[0] + _A[1]*_B[1] + _A[2]*_B[2];\ 

} 

# define VMAG(_A,_B){\ 

10 _B - sqrt(_A[0]*_A[0] + _A[1]*_A[1] + _A[2]*_A[2]);\ 

} 

#define VZERO(_A){\ 

15 _A[0] = 0.0;\ 

_A[1] = 0.0;\ 

_A[2] = 0.0;\ 

} 

20 

#define VCOPY(_A,_B){\ 

-B[0] = -A[0];\ 

-B[i] = -A[i];\ 

25 — B[2] = -A[2];\ 

} 

#define VDIF(_A.J3,_C){\ 

30 _C[0] = -A[0] - _B[0];\ 

-C[l] = -A[l] - -B[l];\ 

-C[2] - -A[2] - JB[2];\ 


# define VADD(-A._B,_C){\ 
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_C[0] = _A[0] + _B[0];\ 

-C[l] = -A[l] + -B[l];\ 

-C[2] = -A [2] + -B [2] ; \ 


#clefine VADD3(_A,_B,_C,_D){\ 
-D[0] = _A[0] + _B[0] + _C[0];\ 
10 -D[l] = _A[1] + _B[l] + _C[l]:\ 

-D [2] = -A [2] + _B[2] + _C[2];\ 

} 

15 # define VSCALAR(_A,_B,_C){\ 
-C[0] = _A[0] * _B;\ 

-C[l] = -A[l] * -B;\ 

_C [2] = _A[2] * _B;\ 


#define VSCALE(_AA,_BB,_CC){\ 
register double _DD;\ 

_DD = _BB;\ 

VSC ALAR( _A A ,_DD,_CC) :\ 

} 

#define VUNIT(_AAA,_BBB){\ 
register double _DDD;\ 

VMAG(_AAA, _DDD);\ 
VSC'ALAR(_AAA, 1.0/.DDD, _BBB):\ 

1 
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/* -CC[i] = _ BB[i][j ] . -AA[j] */ 
#define VROT(_AA,_BB,XC){\ 
VDOT(_AA. J3B[0], _CC[0]);\ 
s VDOT(_AA, _BB[1], _CC[1]);\ 
VDOT(_AA. _BB[2], _CC[2]);\ 

} 


# define VPRINT(_A){ printf("%s = lg, Ig, 7.g\n", 

"_A", -A[0]. _A[1], -A [2]);} 

#dcfine RPRINT(_A){ printf("%s = e /„g\n". "_A", 

15 -A);} 

# define IPRINT(_A){ printf("°/.s = 8 /.d\n", "_A", _A);} 

#define CLIP(_M,_N,_S){\ 

20 if (_S > _M) _S = _M;\ 
else if(_S < _N) _S = _N;\ 

} 

25 . / 

typedef struct { 

Vec3_t n; 

Vec3_t o; 

30 

Vec3_t a; 

Vec3_t p; 

} HTframe_t; 


35 


# define VROTJH[T(JCX,_HH,_YY){\ 
_YY[0] = _HH.n[0]*_YY[0] + 
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_HH.o[0]*_YY[l] + _HH.a[0]*JCX[2];\ 
_YY[1] = _HH.n[l]*_YY[0] + 

_HH.o[l]*_YY[l] + _HH.a[l]*_XX[2];\ 

5 _YY[2] = _HH.n[2]*_YY[0] + 

_HH.o[2]*_YY[l] + _HH.a[2]*_XX[2];\ } 

# define VIROT.HT(_XX._HH,_YY){\ 

10 V D 0 T ( _XX , ( _H H . n ) , _ YY ) ; \ 

VDOT( JXX,(_HH.o) ,_YY) ;\ 

VDOT(_XX,( _HH .a) ,.YY) ;\ 

} 

15 

#define VTRANS_HT( _XX,JHH,-YY){\ 

_YY[0] = _HH.n[0]*_XX[0] + _HH.o[0]*_XX[l] 

+ _HH.a[0]*JvX[2] + _HH.p[0];\ 

20 _YY[1] = _HH.n[l]*_XX[0] + _HH.o[l]*_XX[l] + 

_HH.a[l]*JXX[2] + _HH.p[l];\ 

_YY[2] = _HH.n[2]*_XX[0] + _HH.o[2]*_XX[l] + 
_HH.a[2]*_XX[2] + _HH.p[2];\ } 

25 

#define VITR AXS _HT( _XX,_HH , _YY){\ 
register Vcc3_t _ZZZ:\ 
VDIF(.XX._HH.p,_ZZZ);\ 

VDOT ( _ZZZ,_HH.n._YY[0] ) ;\ 

VD OT( _Z Z Z , _H H .o , _Y Y [ 1 ] ) ; \ 

VD 0T( _ZZ Z , _H H . a. _Y Y [2] ) ; \ 

} a! 


35 



65 


5,602,968 


What is claimed is: 

1. A method of controlling a robot manipulator moving 
toward a target frame F 0 with a target velocity v 0 comprising 
a linear target velocity v with an angular target velocity w 0 

to smoothly and continuously divert said robot manipulator 5 
to a subsequent frame F 1? said target frame being associated 
with a target transition time T 0 and said subsequent frame 
being associated with a subsequent transition time T l9 said 
method comprising the steps of: 

determining a global transition velocity v x necessary to 10 
move said manipulator from said target frame F 0 to said 
subsequent frame F a within said subsequent transition 
time Tj, said global transition velocity comprising a 
linear transition velocity v l and an angular transition 
velocity co^ 

defining a blend time interval 2x 0 within which the global 
velocity of said robot manipulator is to be changed 
from a global target velocity v 0 to said global transition 
velocity v x and dividing said blend time interval 2 t 0 
into discrete time segments 5t; 20 

during each one of said discrete time segments St of said 
blend interval 2x 0 ; 

(a) computing a blended global velocity v of said manipu- 
lator as a blend of said global target velocity v 0 and said 
global transition velocity v l9 said blended global veloc- 25 
ity v being at least approximately equal to said target 
global velocity v 0 at the beginning of said blend time 
interval and at least approximately equal to said global 
transition velocity v x at the end of said blend time 
interval, said blended global velocity v comprising a 30 
blended angular velocity (0 and a blended linear veloc- 
ity v, and 

(b) rotating said manipulator by an incremental rotation 

corresponding to an integration of said blended angular 
velocity co over one discrete time segment St. r: 

2. The method of claim 1 wherein the step of defining a 
blend time interval comprises computing said blend time 
interval 2x 0 from said global target and transition velocities 
v 0 and v x and from a predetermined maximum acceleration 

to which motion of said manipulator is to be limited. 1 

3. The method of claim 2 wherein the step of computing 
said blend time interval comprises dividing a difference 
between said global target and transition velocities v 0 and v x 
by said predetermined maximum acceleration. 

4. The method of claim 2 further comprising a velocity 45 
error correction step carried out about the beginning of said 
blend time interval, said velocity error correction compris- 
ing the steps of: 

determining a desired blend exit frame F 0 , said step of 5Q 
determining comprising rotating said target frame F 0 
through a rotation corresponding to an integration of 
said angular target velocity to over at least a portion of 
said blend time interval 2x 0 ; 

determining an error correction global velocity v e 55 
required to move from said target frame F 0 to said 
desired blend exit frame F' 0 within a correction time 
interval related to said blend time interval; and 

correcting said target velocity by adding to it said error 
correction velocity. 60 

5. The method of claim 4 wherein said portion of said 
blend time interval is about half said blend time interval. 

6. The method of claim 4 wherein said correction time 

interval is a difference between said target transition time 
and half said blend time interval, T 0 -x 0 . 65 

7. The method of claim 4 wherein the step of determining 
a desired blend exit frame F 0 further comprises translating 
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said target frame F 0 by a displacement corresponding to an 
integration of said linear target velocity v over at least a 
portion of said blend time interval 2x 0 . 

8. The method of claim 1 wherein the step of computing 
a blended global velocity comprises computing a sum of 
Vo(l-/)+ v iCf)> wherein f changes with each time increment 
St. 

9. The method of claim 8 wherein f is a function which 
is approximately zero at the beginning of said blend time 
interval and is approximately one at the end of said blend 
time interval to provide linear blending. 

10. The method of claim 8 wherein f is a function which 
provides one of: (a) third order polynomial velocity blend- 
ing, (b) cycloidal velocity blending. 

11. The method of claim 1 further comprising translating 
said manipulator by an incremental translation correspond- 
ing to an integration of said blended linear velocity v over 
one discrete time segment 5t during each of said discrete 
time segments. 

12. The method of claim 1 wherein during a preceding 
time interval immediately prior to said blend time interval 
said manipulator is maintained at an approximately constant 
global velocity equal to said target global velocity v 0 , while 
performing the following steps: 

dividing said preceding time interval into discrete time 
segments 5t; 

during each one of said discrete time segments of said 
preceding time interval, rotating said manipulator by an 
incremental rotation corresponding to an integration of 
said target angular velocity co 0 over one discrete time 
segment St. 

13. The method of claim 12 further comprising translating 
said manipulator by an incremental displacement corre- 
sponding to an integration of said target linear velocity v 0 
over one discrete time segment St. 

14. The method of claim 1 further comprising specifying 
a sequence of successive target frames F z associated with 
respective transition times T f for i between 1 and n wherein 
n is an integer, and wherein after the end of each blend time 
interval said target frame is set to said subsequent frame and 
said subsequent frame is set to a next one of said successive 
frames. 

15. A method of controlling a robot manipulator moving 
toward a target frame F 0 with a target velocity v 0 comprising 
a linear target velocity v with an angular target velocity co 0 
to smoothly and continuously divert said robot manipulator 
to a subsequent frame F 1} said target frame being associated 
with a target transition time T 0 and said subsequent frame 
being associated with a subsequent transition time T 1? said 
method comprising the steps of: 

determining a global transition velocity v x necessary to 
move said manipulator from said target frame F 0 to said 
subsequent frame F x within said subsequent transition 
time T l5 said global transition velocity comprising a 
linear transition velocity v x and an angular transition 
velocity co^ 

defining a blend time interval 2x 0 within which the global 
velocity of said robot manipulator is to be changed 
from a global target velocity v 0 to said global transition 
velocity w 1 and dividing said blend time interval 2x 0 
into discrete time segments 5t; 

during each one of said discrete time segments 5t of said 
blend interval 2x 0 ; 

(a) computing a blended global velocity v of said manipu- 
lator as a blend of said global target velocity v 0 and said 
global transition velocity v x , said blended global veloc- 
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ity v being at least approximately equal to said target 
global velocity v 0 at the beginning of said blend time 
interval and at least approximately equal to said global 
transition velocity Vj at the end of said blend time 
interval, said blended global velocity v comprising a 5 
blended angular velocity co and a blended linear veloc- 
ity v, and 

(b) changing an actual global velocity of said manipulator 
in accordance with said blended angular velocity co. 

16. The method of claim 15 wherein the step of defining 10 
a blend time interval comprises computing said blend time 
interval 2x 0 from said global target and transition velocities 

v 0 and Vj and from a predetermined maximum acceleration 
to which motion of said manipulator is to be limited. 

17. The method of claim 16 wherein the step of computing 15 
said blend time interval comprises dividing a difference 
between said global target and transition velocities v 0 and \ 1 

by said predetermined maximum acceleration. 

18. The method of claim 16 further comprising a velocity 
error correction step carried out about the beginning of said 20 
blend time interval, said velocity error correction compris- 
ing the steps of: 

determining a desired blend exit frame F' 0 , said step of 
determining comprising rotating said target frame F 0 
through a rotation corresponding to an integration of 
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said angular target velocity co over at least a portion of 
said blend time interval 2 t 0 ; 

determining an error correction global velocity v e 
required to move from said target frame F 0 to said 
desired blend exit frame F' 0 within a correction time 
interval related to said blend time interval; and 

correcting said target velocity by adding to it said error 
correction velocity. 

19. The method of claim 18 wherein said portion of said 
blend time interval is about half said blend time interval. 

20. The method of claim 18 wherein said correction time 
interval is a difference between said target transition time 
and half said blend time interval, T 0 -t 0 . 

21. The method of claim 15 wherein the step of computing 
a blended global velocity comprises computing a sum of 
VoO-jD+VjCf), wherein ;f changes with each time increment 
St. 

22. The method of claim 21 wherein f is a function which 
is approximately zero at the beginning of said blend time 
interval and is approximately one at the end of said blend 
time interval to provide linear blending. 

23. The method of claim 21 wherein f is a function which 
provides one of: (a) third order polynomial velocity blend- 
ing, (b) cycloidal velocity blending. 



